-
Notifications
You must be signed in to change notification settings - Fork 991
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
adding mount orientation to mount_control plugin #1297
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Besides the comments, looks good. Could you show us a basic usage of this? Thanks
} | ||
|
||
private: | ||
ros::NodeHandle mount_nh; | ||
ros::Subscriber command_sub; | ||
ros::Publisher mount_orientation_pub; | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
please remove the extra line
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Probably better to apply autoformatter?
ac383d1
to
0f8548e
Compare
*/ | ||
void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &o) | ||
{ | ||
auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(o.roll, o.pitch, o.yaw) * M_PI / 180.0); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I am a little confused on which frame this quaternion represents. The yaw is relative to the vehicle, but roll and pitch is in the global frame. Shouldn't it use yaw_absolute to be more consistent?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Actually, I think it's ok as it is, because it returns the same rpy values that you'd send with MAV_CMD_DO_MOUNT_CONTROL. In our usecase we have a gimbal with a yaw range of -320 to 320 degrees and we would like to get that value, no matter how the drone is positioned.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I understand that the relative yaw values are more useful than the absolute value, but the frame you are publishing as orientation is not properly defined anywhere. What are the two frames orientation q
is actually representing?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for the reminder! According to mavlink description, the roll and pitch and yaw_absolute are according to global frame. roll, pitch and yaw would not be in the same frame. Maybe we can publish this similar to MountControl.msg: as roll, pitch, yaw, yaw_absolute in a MountOrientation.msg and not as quaternion?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@dayjaby I am a little hesitant on having a new custom message definition for that, but I do think that would be less confusing.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Then let's publish roll, pitch and yaw_absolute as a quaternion. The relative yaw can be reconstructed if needed by subtracting the heading (/mavros/global_position/compass_hdg). This might be troublesome though, because PX4 only sends MOUNT_ORIENTATION.yaw but no MOUNT_ORIENTATION.yaw_absolute (see https://github.com/PX4/Firmware/blob/c75954fef8ec6a7c7e4f7a68b2e536c1b5b633ab/src/modules/mavlink/mavlink_messages.cpp#L4722). I will do a PR for that to set yaw_absolute as well. This doesn't matter for gimbals who construct complete MOUNT_ORIENTATION messages, but at least for the typhoon h480 in sitl.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ooh I didn't see that coming. Then I guess doing it as the initial implementation makes sense. I think we just need to be careful not to forget what the yaw means in that case.
@dayjaby Any updates? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You are adding things that are do not bellong to this PR. Please rebase your changes.
66c79b9
to
e4d0172
Compare
Done. |
Nope. Still isn't. Better you close this and create a new PR with a single and clean commit. |
@TSC21 that is not required, it's possible to rebase that branch and then force-push it. Then PR will be updated accordingly. |
b2b5237
to
d82cf35
Compare
d82cf35
to
d9b1dfa
Compare
@TSC21 I am sorry! Been some time since doing PRs. Thanks a lot for your reviews and help. |
@dayjaby Could you resolve the conflicts please? |
Done. |
0759ae2
to
56687ac
Compare
@dayjaby we need a new rebase. Sorry |
150092f
to
c741c21
Compare
@TSC21 Done. |
@dayjaby Github is still complaining that the branch has conflicts... |
@TSC21 "This branch has no conflicts with the base branch" or where can you see that? |
@TSC21 Can we merge this now? |
This PR publishes the gimbal orientation as geometry_msgs/Quaternion in the topic ~mount_control/orientation